
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_ekf.cpp
  * @author     baiyang
  * @date       2021-10-31
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>

#include "ahrs_ekf.h"

#include <EKF/ekf.h>
#include <mavproxy/mavproxy.h>
#include <common/time/gp_time.h>
#include <sensor_imu/sensor_imu.h>
#include <sensor_baro/sensor_baro.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _init();
static bool _update_imu_sample(imuSample &imu_sample_new);
/*----------------------------------variable----------------------------------*/
Ekf *_ekf = nullptr;
parameters *_params = nullptr;

// time slip monitoring
uint64_t _integrated_time_us = 0;    ///< integral of gyro delta time from start (uSec)
uint64_t _start_time_us = 0;         ///< system time at EKF start (uSec)
int64_t  _last_time_slip_us = 0;     ///< Last time slip (uSec)

static bool _had_valid_terrain = false;  ///< true if at any time there was a valid terrain estimate

static itc_node_t vehicle_home_sub;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_init()
{
// 使用keil，不编译ekf，因为keil编译的ekf运行时会报错
// keil 版本：V5.28 编译器 版本：V6.12
#if !(defined(__CC_ARM) || defined(__CLANG_ARM))
    _ekf = new Ekf();
    _params = _ekf->getParamHandle();

    vehicle_home_sub = itc_subscribe(ITC_ID(vehicle_home), NULL);
#endif
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        按100Hz调用
  */
static bool ekf_update_imu_sample(imuSample &imu_sample_new)
{
    bool update = false;
    Vector3f_t delta_angle;
    Vector3f_t delta_velocity;

    uint8_t gyro_index = sensor_imu_get_primary_accel();
    uint8_t accel_index = sensor_imu_get_primary_gyro();

    update = sensor_imu_get_delta_angle(gyro_index, &delta_angle, &imu_sample_new.delta_ang_dt);
    update &= sensor_imu_get_delta_velocity(accel_index, &delta_velocity, &imu_sample_new.delta_vel_dt);

    if (update) {
        imu_sample_new.time_us = sensor_imu_get_last_update_usec();
        imu_sample_new.delta_ang = Vector3f{delta_angle.x, delta_angle.y, delta_angle.z};
        imu_sample_new.delta_vel = Vector3f{delta_velocity.x , delta_velocity.y, delta_velocity.z};
    }

    return update;
}

/**
  * @brief       运行扩展卡尔曼滤波器
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        这个函数的耗时在3ms~15ms之间绝大部分在10ms之内，
  * 目前（20201228）还不清楚为什么耗时会有这么大的波动。
  */
void ahrs_ekf_update()
{
// 使用keil，不编译ekf，因为keil编译的ekf运行时会报错
// keil 版本：V5.28 编译器 版本：V6.12
#if !(defined(__CC_ARM) || defined(__CLANG_ARM))
    bool imu_updated = false;
    static imuSample imu_sample_new {};

    uint64_t imu_dt = 0; // for tracking time slip later

    imu_updated = ekf_update_imu_sample(imu_sample_new);
    imu_dt = imu_sample_new.delta_ang_dt*1e6;

    if (imu_updated) {
        const uint64_t now = imu_sample_new.time_us;
        _ekf->setIMUData(imu_sample_new);

        ahrs_ekf_publish_attitude(now);

        // integrate time to monitor time slippage
        if (_start_time_us > 0) {
            _integrated_time_us += imu_dt;
            _last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;

        } else {
            _start_time_us = imu_sample_new.time_us;
            _last_time_slip_us = 0;
        }
#if 0
        if (itc_poll(pNodeLandDetected)) {
            ItcLanDetected land_detected = {0};
            itc_copy(ITC_ID(VEHICLE_LAND_DETECTED), pNodeLandDetected, &land_detected);

            _ekf->set_in_air_status(!land_detected.landed);
            if (land_detected.armed && (_params->gnd_effect_deadzone > 0.0f)) {
                    if (!_had_valid_terrain) {
                        // update ground effect flag based on land detector state if we've never had valid terrain data
                        _ekf->set_gnd_effect_flag(land_detected.in_ground_effect);
                    }
            } else {
                    _ekf->set_gnd_effect_flag(false);
            }
        }
#endif
        ahrs_ekf_update_baro_sample();
        ahrs_ekf_update_compass_sample();
        ahrs_ekf_update_gps_sample();

        // run the EKF update and output
        const uint64_t ekf_update_start = time_micros64();
        if (_ekf->update()) {
            ahrs_ekf_publish_local_pos(now);
        }
    }
#endif
}
/*------------------------------------test------------------------------------*/


